
//#include "LaborQuat.h"
#include "stdafx.h"

#include "LaborMathInclude.h"
#include "LaborQuat.h"
NAMESPACE_LABOR3D_BEGIN

const float CLQuat::ms_fEpsilon = 1e-03f;
const CLQuat CLQuat::ZERO(0.0,0.0,0.0,0.0);
const CLQuat CLQuat::IDENTITY(1.0,0.0,0.0,0.0);

//-----------------------------------------------------------------------
void CLQuat::fromRotationMatrix (const CLMatrix3x3& kRot)
{
	// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
	// article "CLQuat Calculus and Fast Animation".

	float fTrace = kRot[0][0]+kRot[1][1]+kRot[2][2];
	float fRoot;

	if ( fTrace > 0.0f )
	{
		// |w| > 1/2, may as well choose w > 1/2
		fRoot = fastSqrt(fTrace + 1.0f);  // 2w
		w = 0.5f*fRoot;
		fRoot = 0.5f/fRoot;  // 1/(4w)
		x = (kRot[2][1]-kRot[1][2])*fRoot;
		y = (kRot[0][2]-kRot[2][0])*fRoot;
		z = (kRot[1][0]-kRot[0][1])*fRoot;
	}
	else
	{
		// |w| <= 1/2
		static int s_iNext[3] = { 1, 2, 0 };
		int i = 0;
		if ( kRot[1][1] > kRot[0][0] )
			i = 1;
		if ( kRot[2][2] > kRot[i][i] )
			i = 2;
		int j = s_iNext[i];
		int k = s_iNext[j];

		fRoot = fastSqrt(kRot[i][i]-kRot[j][j]-kRot[k][k] + 1.0f);
		float* apkQuat[3] = { &x, &y, &z };
		*apkQuat[i] = 0.5f*fRoot;
		fRoot = 0.5f/fRoot;
		w = (kRot[k][j]-kRot[j][k])*fRoot;
		*apkQuat[j] = (kRot[j][i]+kRot[i][j])*fRoot;
		*apkQuat[k] = (kRot[k][i]+kRot[i][k])*fRoot;
	}
}
//-----------------------------------------------------------------------
void CLQuat::toRotationMatrix (CLMatrix3x3& kRot) const
{
	float fTx  = 2.0f*x;
	float fTy  = 2.0f*y;
	float fTz  = 2.0f*z;
	float fTwx = fTx*w;
	float fTwy = fTy*w;
	float fTwz = fTz*w;
	float fTxx = fTx*x;
	float fTxy = fTy*x;
	float fTxz = fTz*x;
	float fTyy = fTy*y;
	float fTyz = fTz*y;
	float fTzz = fTz*z;

	kRot.m[0][0] = 1.0f-(fTyy+fTzz);
	kRot.m[0][1] = fTxy-fTwz;
	kRot.m[0][2] = fTxz+fTwy;
	kRot.m[1][0] = fTxy+fTwz;
	kRot.m[1][1] = 1.0f-(fTxx+fTzz);
	kRot.m[1][2] = fTyz-fTwx;
	kRot.m[2][0] = fTxz-fTwy;
	kRot.m[2][1] = fTyz+fTwx;
	kRot.m[2][2] = 1.0f-(fTxx+fTyy);
}
//-----------------------------------------------------------------------
void CLQuat::fromAngleAxis (const float& rfAngle,
								const CLPoint3& rkAxis)
{
	// assert:  axis[] is unit length
	//
	// The CLQuat representing the rotation is
	//   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

	float fHalfAngle = 0.5f*rfAngle;
	float fSin = sinf(fHalfAngle);
	w = cosf(fHalfAngle);
	x = fSin*rkAxis.x;
	y = fSin*rkAxis.y;
	z = fSin*rkAxis.z;
}
//-----------------------------------------------------------------------
void CLQuat::toAngleAxis (float& rfAngle, CLPoint3& rkAxis) const
{
	// The CLQuat representing the rotation is
	//   q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)

	float fSqrLength = x*x+y*y+z*z;
	if ( fSqrLength > 0.0f )
	{
		rfAngle = 2.0f*acosf(w);
		float fInvLength = invSqrt(fSqrLength);
		rkAxis.x = x*fInvLength;
		rkAxis.y = y*fInvLength;
		rkAxis.z = z*fInvLength;
	}
	else
	{
		// angle is 0 (mod 2*pi), so any axis will do
		rfAngle = 0.0f;
		rkAxis.x = 1.0f;
		rkAxis.y = 0.0f;
		rkAxis.z = 0.0f;
	}
}
//-----------------------------------------------------------------------
void CLQuat::fromAxes (const CLPoint3* akAxis)
{
	CLMatrix3x3 kRot;

	for (int iCol = 0; iCol < 3; iCol++)
	{
		kRot.m[0][iCol] = akAxis[iCol].x;
		kRot.m[1][iCol] = akAxis[iCol].y;
		kRot.m[2][iCol] = akAxis[iCol].z;
	}

	fromRotationMatrix(kRot);
}
//-----------------------------------------------------------------------
void CLQuat::fromAxes (const CLPoint3& xAxis, const CLPoint3& yAxis, const CLPoint3& zAxis)
{
	CLMatrix3x3 kRot;

	kRot.m[0][0] = xAxis.x;
	kRot.m[1][0] = xAxis.y;
	kRot.m[2][0] = xAxis.z;

	kRot.m[0][1] = yAxis.x;
	kRot.m[1][1] = yAxis.y;
	kRot.m[2][1] = yAxis.z;

	kRot.m[0][2] = zAxis.x;
	kRot.m[1][2] = zAxis.y;
	kRot.m[2][2] = zAxis.z;

	fromRotationMatrix(kRot);

}
//-----------------------------------------------------------------------
void CLQuat::toAxes (CLPoint3* akAxis) const
{
	CLMatrix3x3 kRot;

	toRotationMatrix(kRot);

	for (int iCol = 0; iCol < 3; iCol++)
	{
		akAxis[iCol].x = kRot[0][iCol];
		akAxis[iCol].y = kRot[1][iCol];
		akAxis[iCol].z = kRot[2][iCol];
	}
}
//-----------------------------------------------------------------------
CLPoint3 CLQuat::xAxis(void)
{
	float fTx  = 2.0f*x;
	float fTy  = 2.0f*y;
	float fTz  = 2.0f*z;
	float fTwy = fTy*w;
	float fTwz = fTz*w;
	float fTxy = fTy*x;
	float fTxz = fTz*x;
	float fTyy = fTy*y;
	float fTzz = fTz*z;

	return CLPoint3(1.0f-(fTyy+fTzz), fTxy+fTwz, fTxz-fTwy);
}
//-----------------------------------------------------------------------
CLPoint3 CLQuat::yAxis(void)
{
	float fTx  = 2.0f*x;
	float fTy  = 2.0f*y;
	float fTz  = 2.0f*z;
	float fTwx = fTx*w;
	float fTwz = fTz*w;
	float fTxx = fTx*x;
	float fTxy = fTy*x;
	float fTyz = fTz*y;
	float fTzz = fTz*z;

	return CLPoint3(fTxy-fTwz, 1.0f-(fTxx+fTzz), fTyz+fTwx);
}
//-----------------------------------------------------------------------
CLPoint3 CLQuat::zAxis(void)
{
	float fTx  = 2.0f*x;
	float fTy  = 2.0f*y;
	float fTz  = 2.0f*z;
	float fTwx = fTx*w;
	float fTwy = fTy*w;
	float fTxx = fTx*x;
	float fTxz = fTz*x;
	float fTyy = fTy*y;
	float fTyz = fTz*y;

	return CLPoint3(fTxz+fTwy, fTyz-fTwx, 1.0f-(fTxx+fTyy));
}
//-----------------------------------------------------------------------
void CLQuat::toAxes (CLPoint3& xAxis, CLPoint3& yAxis, CLPoint3& zAxis) const
{
	CLMatrix3x3 kRot;

	toRotationMatrix(kRot);

	xAxis.x = kRot[0][0];
	xAxis.y = kRot[1][0];
	xAxis.z = kRot[2][0];

	yAxis.x = kRot[0][1];
	yAxis.y = kRot[1][1];
	yAxis.z = kRot[2][1];

	zAxis.x = kRot[0][2];
	zAxis.y = kRot[1][2];
	zAxis.z = kRot[2][2];
}

//-----------------------------------------------------------------------
CLQuat CLQuat::operator+ (const CLQuat& rkQ) const
{
	return CLQuat(w+rkQ.w,x+rkQ.x,y+rkQ.y,z+rkQ.z);
}
//-----------------------------------------------------------------------
CLQuat CLQuat::operator- (const CLQuat& rkQ) const
{
	return CLQuat(w-rkQ.w,x-rkQ.x,y-rkQ.y,z-rkQ.z);
}
//-----------------------------------------------------------------------
CLQuat CLQuat::operator* (const CLQuat& rkQ) const
{
	// NOTE:  Multiplication is not generally commutative, so in most
	// cases p*q != q*p.

	return CLQuat
		(
		w * rkQ.w - x * rkQ.x - y * rkQ.y - z * rkQ.z,
		w * rkQ.x + x * rkQ.w + y * rkQ.z - z * rkQ.y,
		w * rkQ.y + y * rkQ.w + z * rkQ.x - x * rkQ.z,
		w * rkQ.z + z * rkQ.w + x * rkQ.y - y * rkQ.x
		);
}
//-----------------------------------------------------------------------
CLQuat CLQuat::operator* (float fScalar) const
{
	return CLQuat(fScalar*w,fScalar*x,fScalar*y,fScalar*z);
}
//-----------------------------------------------------------------------
//CLQuat operator* (float fScalar, const CLQuat& rkQ)
//{
//}
//-----------------------------------------------------------------------
CLQuat CLQuat::operator- () const
{
	return CLQuat(-w,-x,-y,-z);
}
//-----------------------------------------------------------------------
float CLQuat::dot (const CLQuat& rkQ) const
{
	return w*rkQ.w+x*rkQ.x+y*rkQ.y+z*rkQ.z;
}
//-----------------------------------------------------------------------
float CLQuat::norm () const
{
	return w*w+x*x+y*y+z*z;
}
//-----------------------------------------------------------------------
CLQuat CLQuat::inverse () const
{
	float fNorm = w*w+x*x+y*y+z*z;
	if ( fNorm > 0.0f )
	{
		float fInvNorm = 1.0f/fNorm;
		return CLQuat(w*fInvNorm,-x*fInvNorm,-y*fInvNorm,-z*fInvNorm);
	}
	else
	{
		// return an invalid result to flag the error
		return ZERO;
	}
}
//-----------------------------------------------------------------------
CLQuat CLQuat::unitInverse () const
{
	// assert:  'this' is unit length
	return CLQuat(w,-x,-y,-z);
}
//-----------------------------------------------------------------------
CLQuat CLQuat::exp () const
{
	// If q = A*(x*i+y*j+z*k) where (x,y,z) is unit length, then
	// exp(q) = cos(A)+sin(A)*(x*i+y*j+z*k).  If sin(A) is near zero,
	// use exp(q) = cos(A)+A*(x*i+y*j+z*k) since A/sin(A) has limit 1.

	float fAngle = fastSqrt(x*x+y*y+z*z);
	float fSin = sinf(fAngle);

	CLQuat kResult;
	kResult.w = cosf(fAngle);

	if ( abs(fSin) >= ms_fEpsilon )
	{
		float fCoeff = fSin/fAngle;
		kResult.x = fCoeff*x;
		kResult.y = fCoeff*y;
		kResult.z = fCoeff*z;
	}
	else
	{
		kResult.x = x;
		kResult.y = y;
		kResult.z = z;
	}

	return kResult;
}
//-----------------------------------------------------------------------
CLQuat CLQuat::log () const
{
	// If q = cos(A)+sin(A)*(x*i+y*j+z*k) where (x,y,z) is unit length, then
	// log(q) = A*(x*i+y*j+z*k).  If sin(A) is near zero, use log(q) =
	// sin(A)*(x*i+y*j+z*k) since sin(A)/A has limit 1.

	CLQuat kResult;
	kResult.w = 0.0f;

	if ( abs(w) < 1.0f )
	{
		float fAngle = acosf(w);
		float fSin = sinf(fAngle);
		if ( abs(fSin) >= ms_fEpsilon )
		{
			float fCoeff = fAngle/fSin;
			kResult.x = fCoeff*x;
			kResult.y = fCoeff*y;
			kResult.z = fCoeff*z;
			return kResult;
		}
	}

	kResult.x = x;
	kResult.y = y;
	kResult.z = z;

	return kResult;
}
//-----------------------------------------------------------------------
CLPoint3 CLQuat::operator* (const CLPoint3& v) const
{
	// nVidia SDK implementation
	CLPoint3 uv, uuv; 
	CLPoint3 qvec(x, y, z);
	uv = qvec.cross(v); 
	uuv = qvec.cross(uv); 
	uv *= (2.0f * w); 
	uuv *= 2.0f; 

	return v + uv + uuv;

}
//-----------------------------------------------------------------------
CLQuat CLQuat::slerp (float fT, const CLQuat& rkP,
							  const CLQuat& rkQ, bool shortestPath)
{
	float fCos = rkP.dot(rkQ);
	float fAngle = acos(fCos);

	if ( abs(fAngle) < ms_fEpsilon )
		return rkP;

	float fSin = sinf(fAngle);
	float fInvSin = 1.0f/fSin;
	float fCoeff0 = sinf((1.0f-fT)*fAngle)*fInvSin;
	float fCoeff1 = sinf(fT*fAngle)*fInvSin;
	// Do we need to invert rotation?
	if (fCos < 0.0f && shortestPath)
	{
		fCoeff0 = -fCoeff0;
		// taking the complement requires renormalisation
		CLQuat t(fCoeff0*rkP + fCoeff1*rkQ);
		t.normalise();
		return t;
	}
	else
	{
		return fCoeff0*rkP + fCoeff1*rkQ;
	}
}
//-----------------------------------------------------------------------
CLQuat CLQuat::slerpExtraSpins (float fT,
										const CLQuat& rkP, const CLQuat& rkQ, int iExtraSpins)
{
	float fCos = rkP.dot(rkQ);
	float fAngle = acos(fCos);

	if ( abs(fAngle) < ms_fEpsilon )
		return rkP;

	float fSin = sinf(fAngle);
	float fPhase = PI*iExtraSpins*fT;
	float fInvSin = 1.0f/fSin;
	float fCoeff0 = sinf((1.0f-fT)*fAngle - fPhase)*fInvSin;
	float fCoeff1 = sinf(fT*fAngle + fPhase)*fInvSin;
	return fCoeff0*rkP + fCoeff1*rkQ;
}
//-----------------------------------------------------------------------
void CLQuat::intermediate (const CLQuat& rkQ0,
							   const CLQuat& rkQ1, const CLQuat& rkQ2,
							   CLQuat& rkA, CLQuat& rkB)
{
	// assert:  q0, q1, q2 are unit zQuats

	CLQuat kQ0inv = rkQ0.unitInverse();
	CLQuat kQ1inv = rkQ1.unitInverse();
	CLQuat rkP0 = kQ0inv*rkQ1;
	CLQuat rkP1 = kQ1inv*rkQ2;
	CLQuat kArg = 0.25f*(rkP0.log()-rkP1.log());
	CLQuat kMinusArg = -kArg;

	rkA = rkQ1*kArg.exp();
	rkB = rkQ1*kMinusArg.exp();
}
//-----------------------------------------------------------------------
CLQuat CLQuat::squad (	float fT,
						const CLQuat& rkP, const CLQuat& rkA,
						const CLQuat& rkB, const CLQuat& rkQ, bool shortestPath)
{
	float fSlerpT = 2.0f*fT*(1.0f-fT);
	CLQuat kSlerpP = slerp(fT, rkP, rkQ, shortestPath);
	CLQuat kSlerpQ = slerp(fT, rkA, rkB);
	return slerp(fSlerpT, kSlerpP ,kSlerpQ);
}
//-----------------------------------------------------------------------
float CLQuat::normalise(void)
{
	float len = norm();
	float factor = 1.0f / fastSqrt(len);
	*this = *this * factor;
	return len;
}


NAMESPACE_LABOR3D_END